#include "MY_INC\PHY\PHY_Cordic.h"

// 输入的角度为归一化角度[0~1]<-->[0~2*pi]
void value_to_cordic31(float *value, int32_t *cordic31, uint8_t Length)
{
    for (uint8_t i = 0; i < Length; i++)
    {
        if (value[i] > 0.5f) // 将角度从[0, 1.0]转换到[-0.5, 0.5]
            value[i] -= 1.0f;

        cordic31[i] = (int32_t)((value[i]) * 0x100000000);
    }
}

void cordic31_to_value(int32_t *cordic31, float *res, uint8_t Length)
{
    for (uint8_t i = 0; i < Length; i++)
    {
        res[i] = (float)cordic31[i] / 0x80000000;
    }
}

void My_Cordic_Calculate(int32_t *Input_Data, int32_t *Output_Data, uint8_t Length)
{
    uint8_t j = 0;
    for (uint8_t i = 0; i < Length; i++)
    {
        CORDIC->WDATA = Input_Data[i];
        Output_Data[j] = CORDIC->RDATA;
        j++;
        Output_Data[j] = CORDIC->RDATA;
        j++;
    }
}

void Cordic_SinCos(float theta, float *sindata, float *cosdata)
{
	theta*=0.15915494309189f;
    if (theta > 0.5f) // 将角度从[0, 1.0]转换到[-0.5, 0.5]
        theta -= 1.0f;
    CORDIC->WDATA = (int32_t)((theta) * 0x100000000);
    *sindata = (float)CORDIC->RDATA * 0.00000000046566128730773925f;
    /*解释一下为什么这么写，在[Π/2,3*Π/2]区间内CORDIC->RDATA的值被抬高了(0x80000000<<1)，正常来说都不会大于1*/
    /*大于0x80000000那么就把多的部分减掉*/
    int32_t RDATA_temp = (CORDIC->RDATA > 0x80000000) ? CORDIC->RDATA - (0x80000000 << 1) : CORDIC->RDATA;
    *cosdata = (float)RDATA_temp * 0.00000000046566128730773925f;
}
// 特供版CORDIC_SinCos函数，直接传入弧度值，返回sin和cos值
void Cordic_SinCos_Init()
{
	LL_CORDIC_Config(CORDIC, CORDIC_FUNCTION_SINE, // sin模式
	                 CORDIC_PRECISION_6CYCLES,     // 6个迭代周�?????????????????
	                 CORDIC_SCALE_0,               // 比例因子，无比例
	                 CORDIC_NBWRITE_1,             // 输入为一�?????????????????32bit�?????????????????
	                 CORDIC_NBREAD_2,              // 输出为两�?????????????????32bit�?????????????????
	                 CORDIC_INSIZE_32BITS,         // 输入�?????????????????32bit格式，Q31
	                 CORDIC_OUTSIZE_32BITS);       // 输出�?????????????????32bit格式，Q31
}

